/* ######################################################################### */
/** \file CompareLaser.hpp
 * \brief	This file contains the function that compare two laser
 *          or distance images and decides if the difference between them
 *          is enough to obtain dynamic objects
 *
 *
 * PROJECT:   DATMO ROS NODE
 *
 * Copyright (c) 2011 CAR - Universidad Politécnica de Madrid.
 *
 * \author    Gonzalo Rodriguez \n
 *            Universidad Politécnica de Madrid \n
 *            Grupo de Robótica y Cibernética \n
 * \version   0.01
 * \date      2011-06-23
 *
 * \n \n
 * Versionshistory: \n
 * -----------------
 * - Version 0.01:   Gonzalo RODRIGUEZ         2011-06-23 \n
 *      First implementation CompareLaser.
 *
 ######################################################################### */


#include <math.h>
/* ------------------------------------------------------------------------- */
/**	\namespace DATMO
 *
 *  \brief	This is the namespace of the DATMO ROS NODE
 */
/* ------------------------------------------------------------------------- */
namespace DATMO
{
/* ------------------------------------------------------------------------- */
/**	\class CompareLaser
 *
 *  \brief  This is the class of CompareLaserMeasurement function.
 *
 *
 */
/* ------------------------------------------------------------------------- */
class cl_CompareLaser
{
public:
	/* ------------------------------------------------------------------------- */
	/**	\fn public static void CompareLaser()
	 *
	 *  \brief
	 *
	 *  \param[in]
	 *  \param[in]
	 *
	 *  \param[out] bool
	 *
	 */
	/* ------------------------------------------------------------------------- */
	static bool CompareLaser(DistanceImage L1, DistanceImage L2)
	{
		bool b_isEqual = true;
		if (L1.s_width == L2.s_width && L1.s_height == L2.s_height)
		{
			for (int i = 1; i <= L1.s_height; i++)
			{
				for (int j = 1; j <= L1.s_width; j++)
				{
					if (fabs(L1.tf_Distance(i,j) - L2.tf_Distance(i, j))>0.00008)
					{ b_isEqual = false; break; }
				}
				if (!b_isEqual)
					break;
			}
		}
		else
		{
			b_isEqual = false;
		}

		return b_isEqual;
	};
};
}
